\documentclass[a4paper,12pt]{article}
\usepackage{graphicx}
\usepackage{mathtools}
\usepackage{algorithmic}
\usepackage{algorithm}
\usepackage{tikz}
\usetikzlibrary{positioning,arrows}
\DeclareMathOperator*{\argmax}{arg\,max}
\begin{document}
\title{Restaurant Delivery Robots (RDR)}
\author{Tang Xiao (U095395W)\\Huang Qi (A0079962L)\\Chong Chee Yong (A0080053U)}
\maketitle
\begin{abstract}
In our project, we apply motion planning algorithms to Restaurant Delivery Robots (RDR), to enable them to deliver food like human waiters. To be more specific, after receiving a task, RDR goes to the food collection point, then routes to specific customers with the ability to avoid static as well as dynamic obstacles, and then returns to standby point. Also to make sure food like soup would not be spilled, speed of RDR is constrained. We also consider the case of slippery floor due to water spills, and RDR would attempt to avoid them if possible.
\end{abstract}

\section{Problem}
A Restaurant Delivery Robot (RDR) is used to deliver and serve food to customers at the tables. The problems, after receiving delivery commands, that this robot will attempt to solve include
\begin{itemize}
  \item Determine a goal position near the target table to serve food.
  \item Determine a valid path to the table, avoid static osbtacles (tables), and avoid water spills when possible. That is because
    \begin{itemize}
      \item the robot should not collide with any obstacles that will cause damage, and
      \item the robot should avoid moving into water spills, which will spread the water around to create a bigger slip hazard in the restaurant. Additionally, the robot itself may slip and lose control because of the water, so this should be avoided.
    \end{itemize}
  \item Determine the speed in which to move along the path, to avoid hitting into dynamic obstacles (people).
\end{itemize}

\subsection{Restaurant}

\begin{figure}[ht]
\centering
\includegraphics[height=50mm]{simple_start_plan.png}
\caption{A Restaurant with a robot, tables, water spills, and a person.}
\end{figure}

The restaurant contains
\begin{itemize}
  \item Robot (red)
  \item Tables (orange)
  \item Person (green)
  \item Water spills (blue)
\end{itemize}

\subsection{Standby}
The robot starts at the standby position.
This is the position where food is placed onto the robot for delivery.

\subsection{Deliver}
The robot will deliver food to tables.
The goal position is a random position along the perimeter a short
distance away from the target table.

\subsection{Controller}
The controller logic tells the robot what to do.
In this case, the robot alternates between standby and delivery. 
\begin{algorithm}[H]
\caption{Control Loop}
\begin{algorithmic}[1]
  \LOOP
    \STATE generate delivery orders (a list of table goals)
    \STATE deliver (orders)
    \STATE standby
  \ENDLOOP
\end{algorithmic}
\end{algorithm}

\section{Algorithm}
Whether delivering or going to standby, the robot needs to perform planning and execution. The following types of algorithms are used:
\begin{itemize}
  \item Path Planning --- determine the way points to the goal before execution.
  \item Velocity Tuning --- determine how fast to move along path during execution.
\end{itemize}

\subsection{Path Planning}
To plan the path, the robot needs to avoid the walls and tables.
Water spills are also avoided when possible.

Particle RRT \cite{pRRT} is used to perform the path planning.
Standard RRT implies that the robot can move through each path segment with equal probability of success.
The key idea of Particle RRT is to associate a probability of success along the path generated by RRT.
This probability of success is generated by simulating multiple motions from a start point.

\begin{figure}[ht]
\centering
\includegraphics[height=50mm]{simple_start_plan.png}
\caption{Lines denote paths to the goal around (-5,1) being sampled.}
\end{figure}

\begin{figure}[ht]
\centering
\includegraphics[height=50mm]{simple_choose_path.png}
\caption{Green line denote the chosen path to the goal around (-5,1).}
\end{figure}

\begin{figure}[ht]
\centering
\includegraphics[height=50mm]{simple_replan.png}
\caption{After reaching the goal around (5,1), the robot continues to deliver remaining orders.
Because the goal is near, there are green lines in the sample paths
which indicates a probability of success of over 0.5.}
\end{figure}

\begin{figure}[ht]
\centering
\includegraphics[height=50mm]{simple_replan_choose_path.png}
\caption{The best path to the goal is chosen. This is a short path compared to other paths.}
\end{figure}

\begin{algorithm}[H]
\caption{Plan Path}
\begin{algorithmic}[1]
  \STATE $bestProb \leftarrow 0$
  \REPEAT
    \STATE $path \leftarrow$ generate RRT path
    \STATE $prob \leftarrow$ evaluate $path$
    \IF{$prob > bestProb$}
      \STATE $bestProb \leftarrow prob$
      \STATE $bestPath \leftarrow path$
    \ENDIF
    \IF{$prob > threshold$}
      \RETURN $bestPath$
    \ENDIF
  \UNTIL timeout
  \RETURN $bestPath$
\end{algorithmic}
\end{algorithm}

Although water spills are the main problem here, it is possible to apply this to other kinds of spills.


\subsection*{Path Evaluation}
Path is evaluated by sampling the region of each RRT node for the presense of water. This is assume to be like taking a photograph of the particular area of the restaurant, and use some sampling algorithm to determine if the samples contain water.

The probability of success of one node sample is
\[ p_{node} = \left\{
  \begin{array}{ll}
    1 & \text{ if there is no water}\\
    0.5 & \text{ if there is water}
  \end{array} \right. \]

This probability $p_{node}$ is affected by uncertainty (see Sensing Uncertainties).

The probability of success of $N$ node samples is the mean probability
\[ \hat{p_{node}} = \frac{1}{N} \sum_{node} p_{node} \]

The probability of success for the RRT path is the combined probability
\[ p_{path} = \prod_{node} \hat{p_{node}} \]

It is assumed that the water sensor or cameras permeates the entire restaurant, and
thus all probabilities are given equal weight.

\subsection*{Sensing Uncertainties}
In reality, water spills occur randomly. To model such uncertainty, there is a probability of false positive or false negative. Hence it is possible for the robot to touch water.

\section*{Velocity Tuning}
Velocity tuning plans a velocity for moving along the path so that the robot does not collide into moving obstacles (people).
There is a limitation here that the robot could not avoid the dynamic obstacle whose path is in collision with it.

Equations that model the collision dynamics are first constructed, and then a constraint solver \cite{cpvt} is used to determine a feasible velocity. Based on this idea, we have implemented two algorithms. The first one doesn't look ahead, but only tries to find a suitable velocity for the next path segment. Due to limitation of Player/Stage, we are not able to accurately get the robot's time, thus can't smoothly adjust the robot's velocity. So for the second algorithm, we take the whole path into consideration, and tries to find a waiting time $t$ and velocity $v$ such that the robot can avoid dynamic obstacles by traveling at velocity $v$ after waiting for time $t$ at the start.

\begin{algorithm}[H]
\caption{Velocity Tuning 1}
\begin{algorithmic}[1]
  \REPEAT
    \STATE $curr \leftarrow$ get current position
    \STATE $next \leftarrow$ get next waypoint
    \STATE $segment \leftarrow$ pair($curr$, $next$)
	\STATE $V_{max} \leftarrow$ get maximum velocitys
    \STATE $obstacles \leftarrow$ get obstacle positions and velocities
    \STATE $v \leftarrow$ solve constraints ($segment$, $obstacles$,$V_{max}$)
    \STATE set robot velocity to $v$
  \UNTIL goal reached
\end{algorithmic}
\end{algorithm}

\begin{algorithm}[H]
\caption{Velocity Tuning 2}
\begin{algorithmic}[1]
\STATE $path \leftarrow$ get robot path
\STATE $obstacles \leftarrow$ get obstacle positions and velocities 
\STATE $n \leftarrow$ get number of segments in path
\FOR {$i\gets 1, n$}
	\STATE $segment \leftarrow$ $path[i]$
	\STATE $T_{i}^{col} \leftarrow$ solve constraints($segment$,$obstacles$)  \COMMENT{$T_{i}^{col}$ is the interval of collision times at segment $i$}
 \ENDFOR
\STATE $V_{max} \leftarrow$ get max velocity
\STATE $t,v \leftarrow$ findWaitingTimeAndVelocity($T_{i}^{col}$,$V_{max}$)
\STATE set robot sleep time to $t$
\STATE set robot velocity to $v$
 
\end{algorithmic}
\end{algorithm}

\subsection*{Constraints}
The constraints is (quoted from the paper) as follows:

\begin{itemize}
  \item Robot is moving between $V_{i-1} = (x_{i-1},y_{i-1})$ and $V_i = (x_i, y_i)$.
  \item Moving obstacle of size $S_x \times S_y$ is moving between $M_{j-1} = (x_{j-1},y_{j-1})$ and $M_j = (x_j, y_j)$, with velocity $v_j$, starting at time $t_{j-1}$.
\end{itemize}

Let
\begin{itemize}
  \item $(x_r,y_r)$ be the position of the robot,
  \item $(x_o,y_o)$ be the position of the moving obstacle,
  \item $t_c$ the collision time between them,
\end{itemize}
then the constraints for the robot moving on line segment $[V_{i-1},V_i]$, of
equation $a_i \cdot x_r + b_i \cdot c_i = 0$,
\[ \left\{
  \begin{array}{l}
    a_i = y_{i-1} - y_i\\
    b_i = x_i = x_{i-1}\\
    c_i = y_i \cdot x_{i-1} - x_i \cdot y_{i-1}\\
    a_i \cdot x_r + b_i \cdot y_r + c_i = 0\\
    (x_r - x_{i-1}) \cdot (x_r - x_i) \leq 0\\
    (y_r - y_{i-1}) \cdot (y_r - y_i) \leq 0
  \end{array} \right. \]

The moving obstacle moving on line segment $[M_{j-1},M_j]$ at velocity $v_j$, starting at time $t_{j-1}$, with distance traveled $d_j$,
\[ \left\{
  \begin{array}{l}
    d_{j_x} = x_j - x_{j-1}\\
    d_{j_y} = y_j - y_{j-1}\\
    d_j = \sqrt{d_{j_x}^2 + d_{j_y}^2}\\
    v_{j_x} = v_j \cdot (d_{j_x}/d_{j})\\
    v_{j_y} = v_j \cdot (d_{j_y}/d_{j})\\
    x_o = v_{j_x} \cdot (t_c - t_{j-1}) + x_{j-1}\\
    y_o = v_{j_y} \cdot (t_c - t_{j-1}) + y_{j-1}
  \end{array} \right. \]

The collision is the robot within the rectangular zone of the obstacle $S_x \times S_y$,
\[ \left\{
  \begin{array}{l}
    x_r \geq x_o + S_x / 2\\
    x_r \leq x_o - S_x / 2\\
    y_r \geq y_o + S_y / 2\\
    y_r \leq y_o - S_y / 2
  \end{array} \right. \]

\section{Implementation}
The robot and test environments are implemented with the following
\begin{itemize}
  \item C/C++
  \item Player/Stage for execution and visualization of the algorithms.
    However, there are unintended control and sensing uncertainties due to the asynchronous communication between Player/Stage and our Controller.
    This is because Player/Stage is intended to simulate a real robot.
  \item OMPL for path planning
  \item Gecode as the constraint solver for velocity tuning.
\end{itemize}

\section{Experiments}

In addition to seeing how the robot works in a simulated restaurant,
simplified test environments were used to test the algorithms.

\subsection{Parameters Chosen}
The following parameters were chosen
\begin{itemize}
  \item The robot is a square of length 0.5.
  \item The range for RRT is set at 0.5, the same as the robot's length. This will ensure that sampling for water covers the entire path by just sampling at the RRT nodes.
  \item The amount of planning time is 5 seconds. A lower planning time did not allow the robot to always come up with a feasible solution because the map is relatively large compared with the range parameter.
  \item The probability threshold for accepting a path, without further evaluating more paths, is set at 75\%. This is a rare event because the distance between start and goal usually requires a good number of nodes, such that the combined probability of success is usually lower than 50\%. A shorter, thus more optimal, path gives a higher probability of success, and hence the robot tends to choose shorter paths over longer ones.
        
  \item The water sensor error is set at 20\%. Initially it was set at 30\%, but the robot would fail to find water frequently.
  \item The number of samples used is 23. A sample of 13 was used initially, and results were not so good, with quite a high chance of the robot passing through water.
  \item The region that is sampled is square between $[-0.4, 0.4]$ of the RRT node.
  \item The water spill is set to extend a radius of 0.5 from its center.
  \item A moving obstacle (person) is a square of length 0.5.
\end{itemize}

\subsection{Tests for Avoiding Water}
Test environments created for water avoidance requires the robot to
go from one one room to another room with a choice of two corridors.
The following describes the test cases and probability of touching water when compared with RRT.
Each test case is runned 100 times, with alternate starting rooms.
\begin{center}
\begin{tabular}{|c|c|c|c|}
\hline
Case & Test Description & RRT (water) & Particle RRT\\
\hline
1 & Water spills in one corridor & 65\% & 0\% \\
2 & Equal water spills in both corridors& 100\% & 100\% \\
3 & Unequal water spills in both& 90\% & 22\% \\
\hline
\end{tabular}
\end{center}

The result is that the robot is able to avoid water spills better than standard RRT.
Note: There seems to be some form of bias with RRT selecting the watery corridor in test case 1. This may be because one of the corridor is slightly wider.

Also, there is some form of optimization because shorter paths are likely to have higher probabilities of success.

\begin{figure}[ht]
\centering
\includegraphics[height=50mm]{r3_sample.png}
\caption{Water test case 1. Only one corridor contains water. RDR samples both corridors, and tends to choose the upper one.}
\end{figure}

\begin{figure}[ht]
\centering
\includegraphics[height=50mm]{r3a_path.png}
\caption{Water test case 2. Both corridors contain equal amounts of water. RDR will go through either corridor with about equal chance.}
\end{figure}

\begin{figure}[ht]
\centering
\includegraphics[height=50mm]{r3b_path.png}
\caption{Water test case 3. Lower corridor contains more water. RDR tends to move through upper corridor.}
\end{figure}

\subsection{Tests of Velocity Tuning}
The test environment for testing velocity tuning is a cross-road.
The robot goes from left to right of the cross-road; whereas
the dynamic obstacle (person) goes from top to bottom.

The result is the robot will stop and let the person pass when
a collision is imminent, if the person's path can be accurately sensed.

\section{Conclusions}

We have attempted to solve two motion planning problems of Restaurant Delivery Robots.

The first problem is path planning. In path planning, the robot needs to go around water spills so as not to spread the slippery hazard, and possibly not to lose control. For this, we implement the Particle RRT algorithm.

The second problem is velocity planning. In velocity planning, we use velocity tuning to plan how fast the robot should move to avoid hitting dynamic obstacles (people), while following the path. There is a limitation here that the robot could not avoid the dynamic obstacle whose path is in collision with it.

Experiments show that the planned path could avoid water spills, and moving people in a limited environment.
Also, the method of calculating the probabiltiy of success allows the robot to choose short paths among the possible ones.

\begin{thebibliography}{9}

\bibitem{pRRT}
   Nicholas Melchior and Reid Simmons,
   \emph{Particle RRT for Path Planning with Uncertainty},
   2007 IEEE International Conference on Robotics and Automation,
   April, 2007, pp. 1617-1624. 

\bibitem{cpvt}
   Micha\"{e}l Soulignac, Michel Rueher, Patrick Taillibert,
   \emph{A safe and flexible CP-based approach for velocity tuning problems},
   CP'10 Proceedings of the 16th international conference on Principles and practice of constraint programming,
   Pages 628-642 

\end{thebibliography}

\end{document}
